Efficient identification of picking points is critical for automated fruit harvesting. Avocados present unique challenges owing to their irregular shape, weight, and less-structured growing environments, which require specific viewpoints for successful harvesting. We propose a geometry-based, semantics-aware viewpoint-planning algorithm to address these challenges. The planning process involves three key steps: viewpoint sampling, evaluation, and execution. Starting from a partially occluded view, the system first detects the fruit, then leverages geometric information to constrain the viewpoint search space to a 1D circle, and uniformly samples four points to balance the efficiency and exploration. A new picking score metric is introduced to evaluate the viewpoint suitability and guide the camera to the next-best view. We validate our method through simulation against two state-of-the-art algorithms. Results show a 100% success rate in two case studies with significant occlusions, demonstrating the efficiency and robustness of our approach. Our code is available at https://github.com/lineojcd/GSNBV

Reasoning about large numbers of diverse plans to achieve high speed navigation in cluttered environments remains a challenge for robotic systems even in the case of perfect perceptual information. Often, this is tackled by methods that iteratively optimize around a prior seeded trajectory and consequently restrict to local optima. We present a novel planning method using normalizing flows (NFs) to encode expert-styled motion primitives. We also present an accelerated collision checking framework that enables rejecting samples from the prior distribution before running them through the NF model for rapid sampling of collision-free trajectories. The choice of an NF as the generator permits a flexible way to encode diverse multi-modal behavior distributions while maintaining a smooth relation to the input space which allows approximating collision checks on NF inputs rather than outputs. We show comparable performance to model predictive path integral control in random cluttered environments and improved exit rates in a cul-de-sac environment. We conclude by discussing our plans for future work to improve both safety and performance of our controller.

Generating receding-horizon motion trajectories for autonomous vehicles in real-time while also providing safety guarantees is challenging. This is because a future trajectory needs to be planned before the previously computed trajectory is completely executed. This becomes even more difficult if the trajectory is required to satisfy continuous-time collision-avoidance constraints while accounting for a large number of obstacles. To address these challenges, this paper proposes a novel real-time, receding-horizon motion planning algorithm named REachability-based trajectory Design via Exact Formulation of Implicit NEural signed Distance functions (REDEFINED). REDEFINED first applies offline reachability analysis to compute zonotope-based reachable sets that overapproximate the motion of the ego vehicle. During online planning, REDEFINED leverages zonotope arithmetic to construct a neural implicit representation that computes the exact signed distance between a parameterized swept volume of the ego vehicle and obstacle vehicles. REDEFINED then implements a novel, real-time optimization framework that utilizes the neural network to construct a collision avoidance constraint. REDEFINED is compared to a variety of state-of-the-art techniques and is demonstrated to successfully enable the vehicle to safely navigate through complex environments. Code, data, and video demonstrations can be found at https://roahmlab.github.io/redefined/.

Ensuring safe, real-time motion planning in arbitrary environments requires a robotic manipulator to avoid collisions, obey joint limits, and account for uncertainties in the mass and inertia of objects and the robot itself. This paper proposes Autonomous Robust Manipulation via Optimization with Uncertainty-aware Reachability (ARMOUR), a provably-safe, receding-horizon trajectory planner and tracking controller framework for robotic manipulators to address these challenges. ARMOUR first constructs a robust controller that tracks desired trajectories with bounded error despite uncertain dynamics. ARMOUR then uses a novel recursive Newton-Euler method to compute all inputs required to track any trajectory within a continuum of desired trajectories. Finally, ARMOUR over-approximates the swept volume of the manipulator; this enables one to formulate an optimization problem that can be solved in real-time to synthesize provably-safe motions. This paper compares ARMOUR to state of the art methods on a set of challenging manipulation examples in simulation and demonstrates its ability to ensure safety on real hardware in the presence of model uncertainty without sacrificing performance. Project page: https://roahmlab.github.io/armour/.

The Constrained Markov Decision Process (CMDP) formulation allows to solve safety-critical decision making tasks that are subject to constraints. While CMDPs have been extensively studied in the Reinforcement Learning literature, little attention has been given to sampling-based planning algorithms such as MCTS for solving them. Previous approaches perform conservatively with respect to costs as they avoid constraint violations by using Monte Carlo cost estimates that suffer from high variance. We propose Constrained MCTS (C-MCTS), which estimates cost using a safety critic that is trained with Temporal Difference learning in an offline phase prior to agent deployment. The critic limits exploration by pruning unsafe trajectories within MCTS during deployment. C-MCTS satisfies cost constraints but operates closer to the constraint boundary, achieving higher rewards than previous work. As a nice byproduct, the planner is more efficient w.r.t. planning steps. Most importantly, under model mismatch between the planner and the real world, C-MCTS is less susceptible to cost violations than previous work.

Deterministic methods for motion planning guarantee safety amidst uncertainty in obstacle locations by trying to restrict the robot from operating in any possible location that an obstacle could be in. Unfortunately, this can result in overly conservative behavior. Chance-constrained optimization can be applied to improve the performance of motion planning algorithms by allowing for a user-specified amount of bounded constraint violation. However, state-of-the-art methods rely either on moment-based inequalities, which can be overly conservative, or make it difficult to satisfy assumptions about the class of probability distributions used to model uncertainty. To address these challenges, this work proposes a real-time, risk-aware reachability-based motion planning framework called RADIUS. The method first generates a reachable set of parameterized trajectories for the robot offline. At run time, RADIUS computes a closed-form over-approximation of the risk of a collision with an obstacle. This is done without restricting the probability distribution used to model uncertainty to a simple class (e.g., Gaussian). Then, RADIUS performs real-time optimization to construct a trajectory that can be followed by the robot in a manner that is certified to have a risk of collision that is less than or equal to a user-specified threshold. The proposed algorithm is compared to several state-of-the-art chance-constrained and deterministic methods in simulation, and is shown to consistently outperform them in a variety of driving scenarios. A demonstration of the proposed framework on hardware is also provided.

There are many industrial, commercial and social applications for multi-agent planning for multirotors such as autonomous agriculture, infrastructure inspection and search and rescue. Thus, improving on the state-of-the-art of multi-agent planning to make it a viable real-world solution is of great benefit. In this work, we propose a new method for multi-agent planning in a static environment that improves our previous work by making it fully online as well as robust to communication latency. The proposed framework generates a global path and a Safe Corridor to avoid static obstacles in an online fashion (generated offline in our previous work). It then generates a time-aware Safe Corridor which takes into account the future positions of other agents to avoid intra-agent collisions. The time-aware Safe Corridor is given with a local reference trajectory to an MIQP (Mixed-Integer Quadratic Problem)/MPC (Model Predictive Control) solver that outputs a safe and optimal trajectory. The planning frequency is adapted to account for communication delays. The proposed method is fully online, real-time, decentralized, and synchronous. It is compared to 3 recent state-of-the-art methods in simulations. It outperforms all methods in robustness and safety as well as flight time. It also outperforms the only other state-of-the-art latency robust method in computation time.

Autonomous agents that drive on roads shared with human drivers must reason about the nuanced interactions among traffic participants. This poses a highly challenging decision making problem since human behavior is influenced by a multitude of factors (e.g., human intentions and emotions) that are hard to model. This paper presents a decision making approach for autonomous driving, focusing on the complex task of merging into moving traffic where uncertainty emanates from the behavior of other drivers and imperfect sensor measurements. We frame the problem as a partially observable Markov decision process (POMDP) and solve it online with Monte Carlo tree search. The solution to the POMDP is a policy that performs high-level driving maneuvers, such as giving way to an approaching car, keeping a safe distance from the vehicle in front or merging into traffic. Our method leverages a model learned from data to predict the future states of traffic while explicitly accounting for interactions among the surrounding agents. From these predictions, the autonomous vehicle can anticipate the future consequences of its actions on the environment and optimize its trajectory accordingly. We thoroughly test our approach in simulation, showing that the autonomous vehicle can adapt its behavior to different situations. We also compare against other methods, demonstrating an improvement with respect to the considered performance metrics.

Generating safe motion plans in real-time is a key requirement for deploying robot manipulators to assist humans in collaborative settings. In particular, robots must satisfy strict safety requirements to avoid self-damage or harming nearby humans. Satisfying these requirements is particularly challenging if the robot must also operate in real-time to adjust to changes in its environment.This paper addresses these challenges by proposing Reachability-based Signed Distance Functions (RDFs) as a neural implicit representation for robot safety. RDF, which can be constructed using supervised learning in a tractable fashion, accurately predicts the distance between the swept volume of a robot arm and an obstacle. RDF's inference and gradient computations are fast and scale linearly with the dimension of the system; these features enable its use within a novel real-time trajectory planning framework as a continuous-time collision-avoidance constraint. The planning method using RDF is compared to a variety of state-of-the-art techniques and is demonstrated to successfully solve challenging motion planning tasks for high-dimensional systems faster and more reliably than all tested methods.